#ifndef STATE_H
#define STATE_H

#include <../Converter.h>

namespace msf
{

constexpr double kG = 9.7921; // constexpr的作用是用来修饰一些函数或变量，使其成为常量表达式，从而在编译器就可以进行计算，进一步提高程序运行期的效率

constexpr int kStateDim = 15;
constexpr int kNoiseDim = 12;

/**
 * @brief local or global angular error or rotation perturbation, ref: JoanSola ESKF 7.
 *
 * @details residual, jacobi and state update on rotation with the same methods
 *
 * @ref JoanSola ESKF 7.
 *
 */
enum ANGULAR_ERROR { LOCAL_ANGULAR_ERROR, GLOBAL_ANGULAR_ERROR };

using MatrixSD = Eigen::Matrix<double, kStateDim, kStateDim>;

class State {
 public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      // error-state
      MatrixSD cov;

      // nominal-state
      Eigen::Vector3d p_wb_;
      Eigen::Vector3d v_wb_;
      Eigen::Matrix3d Rwb_;
      Eigen::Vector3d acc_bias;
      Eigen::Vector3d gyr_bias;

      // IMU原始角速度
      Eigen::Vector3f curr_w;

      // Tbo:odom frame to imu frame
      Sophus::SE3<float> Tbo;

      double timestamp;
      double delta_t;

      static ANGULAR_ERROR kAngError;

      State() 
      {
            cov.setZero();

            p_wb_.setZero();
            v_wb_.setZero();
            Rwb_.setIdentity();
            acc_bias.setZero();
            gyr_bias.setZero();
      }

      State(Sophus::SE3<float> &Tbc) 
      {
            cov.setZero();

            p_wb_.setZero();
            v_wb_.setZero();
            Rwb_.setIdentity();
            acc_bias.setZero();
            gyr_bias.setZero();

            Tbo = Tbc;
      }

      void set_bias(const Eigen::Vector3d &ba, const Eigen::Vector3d &bg) 
      {
            acc_bias = ba;
            gyr_bias = bg;
      }

      const Eigen::Isometry3d pose() const { // Isometry3d由一个3x3的旋转矩阵和一个3x1的平移向量组成,可以用于表示三维空间中的刚体运动
            Eigen::Isometry3d Twb;
            Twb.linear() = Rwb_;
            Twb.translation() = p_wb_;
            return Twb;
      }

      void set_pose(const Eigen::Isometry3d &pose) {
            Rwb_ = pose.linear();
            p_wb_ = pose.translation();
      }

      static void update_pose(Eigen::Isometry3d &pose, const Eigen::Matrix<double, 6, 1> &delta_pose) {
            pose.translation().noalias() += delta_pose.head(3); // 用于表示在赋值操作时，不考虑目标表达式和源表达式之间的别名问题。别名问题是指在赋值操作时，目标表达式和源表达式有重叠的内存区域，导致赋值结果出错。
            pose.linear() = rotation_update(pose.rotation(), delta_rot_mat(delta_pose.tail(3)));
      }

      // 将p和R转换为四元数表示
      const Eigen::Matrix<double, 7, 1> vec_pq() const {
            Eigen::Matrix<double, 7, 1> vec;

            vec.head(3) = p_wb_;
            const auto &q = Eigen::Quaterniond(Rwb_);
            vec.segment<3>(3) = q.vec();
            vec(6) = q.w();

            return vec;
      }

      // 四元数表示转换为p和R
      const void set_vec_pq(const Eigen::Matrix<double, 7, 1> &vec_pq) {
            p_wb_ = vec_pq.head(3);
            Eigen::Quaterniond q;
            q.vec() = vec_pq.segment<3>(3);
            q.w() = vec_pq(6);
            Rwb_ = q.toRotationMatrix();
            // Rwb_ = Eigen::Quaterniond(vec_pq.tail(4).data()).toRotationMatrix();  // xyzw
      }

      // 将v和ba、bg转换为matrix
      const Eigen::Matrix<double, 9, 1> vec_vb() const {
            Eigen::Matrix<double, 9, 1> vec;

            vec.segment<3>(0) = v_wb_;
            vec.segment<3>(3) = acc_bias;
            vec.segment<3>(6) = gyr_bias;

            return vec;
      }

      // 将matrix转换为v、ba和bg
      const void set_vec_vb(const Eigen::Matrix<double, 9, 1> &vec_vb) {
            v_wb_ = vec_vb.segment<3>(0);
            acc_bias = vec_vb.segment<3>(3);
            gyr_bias = vec_vb.segment<3>(6);
      }

      // 将p v q ba bg转换为vec
      const Eigen::Matrix<double, kStateDim, 1> vec() const {
            Eigen::Matrix<double, kStateDim, 1> vec;

            vec.segment<3>(0) = p_wb_;
            vec.segment<3>(3) = v_wb_;
            vec.segment<3>(6) = Converter::rot_mat_to_vec(Rwb_);
            vec.segment<3>(9) = acc_bias;
            vec.segment<3>(12) = gyr_bias;

            return vec;
      }

      // 将vec转换为p v q ba bg
      void from_vec(const Eigen::Matrix<double, kStateDim, 1> &vec) {
            p_wb_ = vec.segment<3>(0);
            v_wb_ = vec.segment<3>(3);
            Rwb_ = Converter::rot_vec_to_mat(vec.segment<3>(6));
            acc_bias = vec.segment<3>(9);
            gyr_bias = vec.segment<3>(12);
      }

      /**
       * @brief Set the cov object
       *
       * @param sigma_p pos std, m
       * @param sigma_v vel std, m/s
       * @param sigma_rp roll pitch std
       * @param sigma_yaw yaw std
       * @param sigma_ba Acc bias
       * @param sigma_bg Gyr bias
       */
      void set_cov(double sigma_p, double sigma_v, double sigma_rp, double sigma_yaw, double sigma_ba, double sigma_bg) {
            cov.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * sigma_p * sigma_p;
            cov.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * sigma_v * sigma_v;
            cov.block<2, 2>(6, 6) = Eigen::Matrix2d::Identity() * sigma_rp * sigma_rp;
            cov(8, 8) = sigma_yaw * sigma_yaw;
            cov.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * sigma_ba * sigma_ba;
            cov.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity() * sigma_bg * sigma_bg;
      }

      State &operator=(const State &rhs) { // 函数将返回对象的引用，而不是对象本身的副本。这样做可以避免在函数调用期间进行对象复制，从而提高效率并减少内存使用
            if (this == &rhs) return *this;  // 一个函数返回一个对象时，如果该对象是一个左值，那么可以返回它的引用；如果该对象是一个右值，则应该将其作为值返回，而不是引用
            p_wb_ = rhs.p_wb_;
            v_wb_ = rhs.v_wb_;
            Rwb_ = rhs.Rwb_;
            acc_bias = rhs.acc_bias;
            gyr_bias = rhs.gyr_bias;
            return *this;
      }      

      // 重载的加法运算符，用于对State类的对象进行加法操作. x_opt = x + delta_x
      State operator+(const Eigen::Matrix<double, kStateDim, 1> &delta_x) const { // 返回值被复制到临时变量中并传递给调用者。这个临时变量是一个右值，因为它不能被修改。由于右值无法被修改，因此引用的用处是非常有限的
            State state;
            state.p_wb_ = this->p_wb_ + delta_x.block<3, 1>(0, 0);
            state.v_wb_ = this->v_wb_ + delta_x.block<3, 1>(3, 0);
            state.Rwb_ = rotation_update(this->Rwb_, delta_rot_mat(delta_x.block<3, 1>(6, 0)));
            state.acc_bias = this->acc_bias + delta_x.block<3, 1>(9, 0);
            state.gyr_bias = this->gyr_bias + delta_x.block<3, 1>(12, 0);
            return state;
      }

      Eigen::Matrix<double, kStateDim, 1> operator-(const State &rhs) const {
            Eigen::Matrix<double, kStateDim, 1> delta_x;
            delta_x.block<3, 1>(0, 0) = this->p_wb_ - rhs.p_wb_;
            delta_x.block<3, 1>(3, 0) = this->v_wb_ - rhs.v_wb_;
            delta_x.block<3, 1>(6, 0) = rotation_residual(this->Rwb_, rhs.Rwb_);
            delta_x.block<3, 1>(9, 0) = this->acc_bias - rhs.acc_bias;
            delta_x.block<3, 1>(12, 0) = this->gyr_bias - rhs.gyr_bias;
            return delta_x;
      }

      /**
       * @brief
       *
       * @param delta_rot_vec small rotation vector
       * @param flag 0: angle axis, 1: quaternion
       * @return Eigen::Matrix3d
       */
      static Eigen::Matrix3d delta_rot_mat(const Eigen::Vector3d &delta_rot_vec, int flag = 0) {
            Eigen::Matrix3d deltaR = Eigen::Matrix3d::Identity();
            if (flag == 0 && delta_rot_vec.norm() > DBL_EPSILON) { // DBL_EPSILON是一个宏，表示double类型所能识别的最小精度. 2.pow(-52)
                  deltaR = Converter::rot_vec_to_mat(delta_rot_vec);
            }
            if (flag == 1) {
                  Eigen::Quaterniond delta_q;
                  delta_q.w() = 1;
                  delta_q.vec() = 0.5 * delta_rot_vec;
                  deltaR = delta_q.toRotationMatrix();
            }
            return deltaR;
      }

      /**
       * @brief Rwb + delta_rot_mat
       *
       * @details Rwb: 1) active 2) local-to-global
       *
       * @param Rwb
       * @param delta_rot_mat small rotation matrix, local or global perturbation
       * @return Eigen::Matrix3d
       */
      static Eigen::Matrix3d rotation_update(const Eigen::Matrix3d &Rwb, const Eigen::Matrix3d &delta_rot_mat) {
            Eigen::Matrix3d updatedR = Eigen::Matrix3d::Identity();
            switch (kAngError) {
                  case ANGULAR_ERROR::LOCAL_ANGULAR_ERROR:
                        updatedR = Rwb * delta_rot_mat;
                        break;
                  case ANGULAR_ERROR::GLOBAL_ANGULAR_ERROR:
                        updatedR = delta_rot_mat * Rwb;
                        break;
            }
            return updatedR;
      }

      /**
       * @brief Robs - Rest
       *
       * @details Robs, Rest: 1) active 2) local-to-global
       *
       * @param Robs :观测到的旋转矩阵
       * @param Rest :期望的旋转矩阵
       * @return Eigen::Vector3d
       */
      static Eigen::Vector3d rotation_residual(const Eigen::Matrix3d &Robs, const Eigen::Matrix3d &Rest) {
            Eigen::Quaterniond q_res;
            switch (kAngError) {
                  case ANGULAR_ERROR::LOCAL_ANGULAR_ERROR:
                        q_res = Eigen::Quaterniond(Rest.transpose() * Robs); // 表示在Rest的坐标系下，Robs相对于Rest的旋转变换
                        break;
                  case ANGULAR_ERROR::GLOBAL_ANGULAR_ERROR:
                        q_res = Eigen::Quaterniond(Robs * Rest.transpose()); // 在全局坐标系下，Robs相对于Rest的旋转变换
                        break;
            }
            return 2.0 * q_res.vec() / q_res.w();
      }

};
using StatePtr = std::shared_ptr<State>;

// 在类外定义并初始化静态成员变量
//ANGULAR_ERROR State::kAngError = LOCAL_ANGULAR_ERROR;

} // namespace msf
#endif